/*RF24 Basin Transmitter*/

/*Adapted from J Coliz's superb RF24 library*/

/*
 This program is free software; you can redistribute it and/or
 modify it under the terms of the GNU General Public License
 version 2 as published by the Free Software Foundation.
 */

#include <SPI.h>
#include "nRF24L01.h"
#include "RF24.h"
#include "printf.h"

const int steeringInput = A0;  // Analog input pin that the steering potentiometer is attached to
const int speedInput = A1;  // Analog input pin that the speed potentiometer is attached to

const int partidaInput = 2; // Chave de partida
const int partidaLed = 3; // Chave de partida LED
const int farolBaixoInput = 4; // Farol baixo
const int farolBaixoLed = 5; // Farol baixo LED
const int farolAutomaticoInput = 6; // Farol automático
const int farolAutomaticoLed = 9; // Farol automático LED

const int farolAltoInput = 10; // Farol alto
const int farolAltoLed = 11; // Farol alto LED
const int alertaInput = 12; // Alerta
const int alertaLed = A5; // Alerta LED

// Set up nRF24L01 radio on SPI bus plus pins 9 & 10 
RF24 radio(7,8);

// Radio pipe addresses for the 2 nodes to communicate.
const uint64_t pipe = 0xF0F0F0F0E1LL;

//container for the data

int txData[3];

// Discrete commands
int commandsValue = 0;

// Analog values for steering and speed storage
int steeringValue = 0;
int speedValue = 0;

//Connect OK LED Pin

int LEDPin = 13;

void setup(void)
{

  Serial.begin(57600);
  
  printf_begin();
  
  SetupDigitalPins();
    
  SetupTransmitter();
  
  pinMode(LEDPin,OUTPUT);
}

void SetupTransmitter()
{

  // Setup and configure rf radio

  radio.begin();
  
  radio.setDataRate(RF24_2MBPS);//data rate
  
  radio.setPALevel(RF24_PA_MAX); //txn power output

  radio.setRetries(15,15);

  radio.setPayloadSize(32);
  
  radio.setChannel(100); //set the channel to use
  
  radio.openWritingPipe(pipe);
    
  //radio.openReadingPipe(2,pipe);

  radio.startListening();

  radio.printDetails();
}

void SetupDigitalPins()
{
  // Digital inputs
  pinMode(partidaInput,INPUT_PULLUP);
  pinMode(farolBaixoInput,INPUT_PULLUP);
  pinMode(farolAutomaticoInput,INPUT_PULLUP);
  pinMode(farolAltoInput,INPUT_PULLUP);
  pinMode(alertaInput,INPUT_PULLUP);

  // Digital outputs
  pinMode(partidaLed, OUTPUT);
  pinMode(farolBaixoLed, OUTPUT);
  pinMode(farolAutomaticoLed, OUTPUT);
  pinMode(farolAltoLed, OUTPUT);
  pinMode(alertaLed, OUTPUT);
}

boolean TransmitData()
{
    radio.stopListening();
    
    bool ok = radio.write(txData, sizeof(txData));
    
    // Now, continue listening
    radio.startListening();
    
    return ok;

}

int VerifyDigitalInputPins(int commands)
{
  int updatedCommands = commands;
  
  if(!digitalRead(partidaInput)) // Chave de partida
  {
    updatedCommands = updatedCommands | 0x1;
    digitalWrite(partidaLed, HIGH);
    Serial.println("Chave de partida ON!");
  }
  else
  {
    updatedCommands = updatedCommands & 0xFE;
    digitalWrite(partidaLed, LOW);
    Serial.println("Chave de partida OFF!");
  }
  
  if(!digitalRead(farolBaixoInput)) // Farol baixo
  {
    updatedCommands = updatedCommands | 0x2;
    digitalWrite(farolBaixoLed, HIGH);
    Serial.println("Farol baixo ON!");
  }
  else
  {
    updatedCommands = updatedCommands & 0xFD;
    digitalWrite(farolBaixoLed, LOW);
    Serial.println("Faro baixo OFF!");
  }

  if(!digitalRead(farolAutomaticoInput)) // Farol automático
  {
    updatedCommands = updatedCommands | 0x4;
    digitalWrite(farolAutomaticoLed, HIGH);
    Serial.println("Farol automatico ON!");
  }
  else
  {
    updatedCommands = updatedCommands & 0xFB;
    digitalWrite(farolAutomaticoLed, LOW);
    Serial.println("Farol automatico OFF!");
  }
  
  if(!digitalRead(farolAltoInput)) // Farol alto
  {
    updatedCommands = updatedCommands | 0x8;
    digitalWrite(farolAltoLed, HIGH);
    Serial.println("Farol alto ON!");
  }
  else
  {
    updatedCommands = updatedCommands & 0xF7;
    digitalWrite(farolAltoLed, LOW);
    Serial.println("Farol alto OFF!");
  }
  
  if(!digitalRead(alertaInput)) // Alerta
  {
    updatedCommands = updatedCommands | 0x10;
    digitalWrite(alertaLed, HIGH);
    Serial.println("Alerta ON!");
  }
  else
  {
    updatedCommands = updatedCommands & 0xEF;
    digitalWrite(alertaLed, LOW);
    Serial.println("Alerta OFF!");
  }
  
  return updatedCommands;
}

void loop(void)
{
    // Update the data packet
    
    commandsValue = VerifyDigitalInputPins(commandsValue);
    
    txData[0] = commandsValue;
    
    txData[1] = analogRead(steeringInput);
    txData[2] = analogRead(speedInput);
    
//    Serial.print("Steering Value ORIGINAL: ");
//    Serial.println(txData[1]);
//    Serial.print("Speed Value ORIGINAL: ");
//    Serial.println(txData[2]);
    
    if((txData[1] >= 485) && (txData[1] <= 530))
      txData[1] = 511;
    if((txData[2] >= 500) && (txData[2] <= 575))
      txData[2] = 511;
    
//    Serial.print("Steering Value NORMALIZED: ");
//    Serial.println(txData[1]);
//    Serial.print("Speed Value NORMALIZED: ");
//    Serial.println(txData[2]);
//    Serial.print("Steering Value Maped: ");
//    Serial.println(map(txData[1], 0, 1023, 0, 255));
//    Serial.print("Speed Value Maped: ");
//    Serial.println(map(txData[2], 0, 1023, 0, 255));
    
    // Send the Data
    if (TransmitData())
    {
      Serial.println("ok...");
      // Light up a green LED to show that the link is active and well
      digitalWrite(LEDPin,HIGH);
      delay(15);
    }
    else
    {
      Serial.println("failed.");
      // Turn off the green LED to show that the link is down
      digitalWrite(LEDPin,LOW);
      delay(15);
    }
    
    // Turn off the green status LED
    digitalWrite(LEDPin,LOW);

    // Delay for next message
    delay(50);
}

